#include <btl/ViewGeometry/FivePointPose.hpp>
#include <btl/ViewGeometry/DisambiguateEssential.hpp>
#include <btl/ViewGeometry/EssentialMatrix.hpp>
#include <btl/ViewGeometry/PointTriangulation.hpp>
#include <btl/Maths/FloatComparison.hpp>
#include <btl/Maths/GaussJordanElimination.hpp>
#include <btl/Maths/RandomSampler.hpp>

#include "stewenius_test_data.hpp"

#include "TurntableController.hpp"
#include "DrawAxisMarker.hpp"
#include "DrawCameraMarker.hpp"
#include <btl/extra/Gui/ColourMap.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>

// ### WARNING ###
// Storing Eigen::* objects inside std::vector could break things
// due to incorrect alignment (though actually I slightly question
// whether having unaligned storage could cause errors unless
// attempting to actually perform calculations directly on the
// values in the std::vector rather than copying elements out and
// working on them locally)

#include <boost/random.hpp>

#include <GL/glfw.h>

#include <iostream>
#include <sstream>
#include <vector>
#include <string>
#include <algorithm>
#include <iterator>
#include <stdexcept>
#include <limits>
#include <cassert>
#include <cmath>

const int CLOSE_ULPS = 32;
const double CLOSE_TOLERANCE = std::numeric_limits<double>::epsilon() * double(CLOSE_ULPS);
const double RADIANS_PER_DEGREE = M_PI/180.0;
const double DEGREES_PER_RADIAN = 180.0/M_PI;

const double CAMERA_SIZE = 0.3;
const double CAMERA_FOV = 80.0 * RADIANS_PER_DEGREE;

struct World {
   Eigen::Matrix3Xd points;
};

struct Frame {
   Eigen::Isometry3d worldToCamera;
   Eigen::Matrix3Xd measurements; // unit-length ray direction vectors
   std::vector<int> indexes; // indexes of valid measurements
};

struct Sequence {
   std::vector<Frame> frames;
};

struct Motion {
   std::vector<Eigen::Isometry3d> cameraToWorld;
};

struct SimCamera {
   double measurementNoise;
   double fovX;
   int resX;
   int resY;

   SimCamera():
      measurementNoise(0.0),
      fovX(60.0 * RADIANS_PER_DEGREE),
      resX(640), resY(480)
      {}

   void capture(
      Eigen::Matrix3Xd& measurements,
      const Eigen::Isometry3d& P,
      const Eigen::Matrix3Xd& points,
      std::vector<int>& validpoints,
      boost::mt19937& rng) const;
};

World generateWorldUniformBox(int N, const Eigen::Vector3d& minCoord, const Eigen::Vector3d& maxCoord, boost::mt19937& rng) {
   assert(N >= 1);
   boost::uniform_real<> x(minCoord(0), maxCoord(0));
   boost::uniform_real<> y(minCoord(1), maxCoord(1));
   boost::uniform_real<> z(minCoord(2), maxCoord(2));
   
   World W;
   W.points.resize(3, N);
   for (int j = 0; j < N; ++j) {
      // note: must not move these into the call (as in Vector3d(x(rng), y(rng)...)
      // because the order in which function parameters are evaluated is undefined,
      // which means we can't totally guarantee the repeatability of the results
      double xx = x(rng);
      double yy = y(rng);
      double zz = z(rng);
      W.points.col(j) = Eigen::Vector3d(xx, yy, zz);
   }
   return W;
}

Eigen::Isometry3d lerpTransform(double alpha, const Eigen::Isometry3d& a, const Eigen::Isometry3d& b) {
   Eigen::Quaterniond rot0 = Eigen::Quaterniond(a.rotation());
   Eigen::Quaterniond rot1 = Eigen::Quaterniond(b.rotation());
   Eigen::Vector3d pos0 = a.translation();
   Eigen::Vector3d pos1 = b.translation();

   Eigen::Vector3d t = pos0 + alpha*(pos1 - pos0);
   Eigen::Isometry3d pose
      = Eigen::Translation3d(t)
      * Eigen::Isometry3d(rot0.slerp(alpha, rot1));

   return pose;
}

Motion generateLinearMotion(int N, const Eigen::Isometry3d& start, const Eigen::Isometry3d& end) {
   assert(N >= 2);

   Eigen::Quaterniond rot0 = Eigen::Quaterniond(start.rotation());
   Eigen::Quaterniond rot1 = Eigen::Quaterniond(end.rotation());
   Eigen::Vector3d pos0 = start.translation();
   Eigen::Vector3d pos1 = end.translation();

   Motion m;
   for (int i = 0; i < N; ++i) {
      const double alpha = double(i) / double(N-1);

      Eigen::Vector3d t = pos0 + alpha*(pos1 - pos0);

      // interpolate between the start and end poses
      // (linear for the position; spherical-linear for the orientation)
      Eigen::Isometry3d pose
         = Eigen::Translation3d(t)
         * Eigen::Isometry3d(rot0.slerp(alpha, rot1));

      m.cameraToWorld.push_back(pose);
   }
   return m;
}

void SimCamera::capture(
      Eigen::Matrix3Xd& measurements,
      const Eigen::Isometry3d& P,
      const Eigen::Matrix3Xd& points,
      std::vector<int>& validpoints,
      boost::mt19937& rng) const {
   const int N = points.cols();

   measurements.noalias() = P * points;

   const double extentX = tan(fovX / 2.0);
   const double extentY = extentX * (double(resY) / double(resX));
   const double width = extentX*2.0;

   const double noiseSigma
      = measurementNoise * (width / double(resX));

   //std::cout << "noiseSigma = " << noiseSigma << "\n";

   boost::normal_distribution<> noiseDist(0.0, noiseSigma);
   boost::variate_generator<boost::mt19937,boost::normal_distribution<> > noise(rng, noiseDist);

   int scrapped = 0;

   validpoints.clear();
   validpoints.reserve(N);

   // normalise measurements,
   // quantise to the image resolution,
   // apply noise and cut off any measurements ouside the image frame
   for (int j = 0; j < N; ++j) {
      Eigen::Vector3d v = measurements.col(j);
      const double scale = v(2);

      // check for points at infinity, and points behind the camera
      if (scale > CLOSE_TOLERANCE) {
         v /= scale;

         assert(btl::almost_equal(v(2), 1.0));

         // check for points outside the camera frame
         if (v(0) < -extentX || v(0) > extentX)
            v.setZero();
         if (v(1) < -extentY || v(1) > extentY)
            v.setZero();

         if (v(2) > 0.0) {
            if (measurementNoise > 0.0) {
               v(0) += noise();
               v(1) += noise();
            }

            v.normalize();
         }
      } else
         v.setZero();

      if (v(2) == 0.0) {
         ++scrapped;
      } else {
         validpoints.push_back(j);
      }

      //std::cout << "v: " << v.transpose() << "\n";

      measurements.col(j) = v;
   }

   //std::cout << (N - scrapped) << "/" << N << "\n";
}

// capture a frame at a given position
Frame captureFrame(const World& w, const Eigen::Isometry3d& cameraToWorld, const SimCamera& simCamera, boost::mt19937& rng) {
   const int N = w.points.cols();

   Frame f;
   f.worldToCamera = cameraToWorld.inverse();
   simCamera.capture(f.measurements, f.worldToCamera, w.points, f.indexes, rng);

   return f;
}

// capture a sequence, assuming all points are visible in all frames
Sequence captureSequence(const World& w, const Motion& m, const SimCamera& simCamera, boost::mt19937& rng)
{
   Sequence seq;
   seq.frames.reserve(m.cameraToWorld.size());
   for (std::vector<Eigen::Isometry3d>::const_iterator
         poseIt = m.cameraToWorld.begin(); poseIt != m.cameraToWorld.end(); ++poseIt) {

      seq.frames.push_back(Frame());
      seq.frames.back() = captureFrame(w, *poseIt, simCamera, rng);
   }
   return seq;
}

void testGaussJordanEliminationOctave() {
   Eigen::Map<const Eigen::MatrixXd> M(&DATA_M[0][0], 10, 20);
   btl::GaussJordanElimination<Eigen::MatrixXd> gje(M);
   Eigen::MatrixXd B = gje.matrix().block<10,10>(0,10);

   //std::cout << "B:\n" << B.transpose() << "\n";
}

void testBuildEssential(const Sequence& seq) {
   assert(! seq.frames.empty());
   std::cout << "-- TEST: Build essential matrix from Rt\n";

   const Frame& f0 = seq.frames.front();
   const int N = f0.measurements.cols();

   const Eigen::Isometry3d& worldToCamera0 = f0.worldToCamera;

   int totalgood = 0;

   for (std::vector<Frame>::const_iterator
         it = seq.frames.begin(); it != seq.frames.end(); ++it) {
      const Frame& f = *it;

      assert(f.measurements.cols() == f0.measurements.cols());

      Eigen::Isometry3d relPose = worldToCamera0 * f.worldToCamera.inverse();

      Eigen::Matrix3d E = btl::BuildEssentialMatrix(relPose).matrix();
      Eigen::VectorXd v = (f0.measurements.transpose() * E * f.measurements).diagonal();

      int numgood = (v.array().abs() <= CLOSE_TOLERANCE).count();
      totalgood += numgood;
      if (numgood < N)
         std::cout << numgood << "/" << N << " good\n";
   }
   std::cout << totalgood << "/" << (N*seq.frames.size()) << " good\n";
}

void testDecomposeEssential(const Motion& motion) {
   assert(! motion.cameraToWorld.empty());
   std::cout << "-- TEST: Decompose essential matrix to transform\n";

   const Eigen::Isometry3d worldToCamera0 = motion.cameraToWorld.front().inverse();

   int totalgood = 0;

   for (std::vector<Eigen::Isometry3d>::const_iterator
         it = motion.cameraToWorld.begin() + 1; it != motion.cameraToWorld.end(); ++it) {
      const Eigen::Isometry3d& cameraToWorld = *it;

      const Eigen::Isometry3d relPose = worldToCamera0 * cameraToWorld;
      const Eigen::Matrix3d E = btl::BuildEssentialMatrix(relPose).matrix();

      btl::DecomposeEssentialHorn rtE(E);
      const Eigen::Isometry3d Rt[4] = {
         rtE.transformA(),
         rtE.transformB(),
         rtE.transformC(),
         rtE.transformD()
      };

      const Eigen::Vector3d t0 = relPose.translation().normalized();
      const Eigen::Quaterniond q0 = Eigen::Quaterniond(relPose.rotation());

      bool good = false;
      for (int i = 0; !good && i < 4; ++i) {
         const double cosT = t0.dot(Rt[i].translation());

         const Eigen::Quaterniond q = Eigen::Quaterniond(Rt[i].rotation());
         const double angle = q0.angularDistance(q);
         const double cosA = std::cos(angle);

         //std::cout << "[" << i << "] t = " << Rt[i].translation().transpose() << "\n";
         //std::cout << "[" << i << "] cosT = " << cosT << "; angle = " << (angle*DEGREES_PER_RADIAN) << " degrees";

         good = (btl::almost_equal(cosT, 1.0, CLOSE_ULPS) && btl::almost_equal(1.0, cosA, CLOSE_ULPS));

         //std::cout << (good ? " GOOD\n" : "\n");
      }

      if (good)
         ++totalgood;
   }

   std::cout << totalgood << "/" << (motion.cameraToWorld.size() - 1) << " good\n";
}

void testFivePointStewenius(const Sequence& seq, boost::mt19937& rng) {
   const Frame& f0 = seq.frames.front();
   const int N = f0.measurements.cols();

   const int SampleCount = 5000;
   const int SampleSize = 5;

   std::cout << "-- TEST: Stewenius' 5-point pose\n";

   const Eigen::Isometry3d& worldToCamera0 = f0.worldToCamera;

   boost::random_number_generator<boost::mt19937> std_rng(rng);

   btl::FivePointStewenius fivePt;
   std::vector<int> order;
   order.reserve(N);
   for (int i = 0; i < N; ++i)
      order.push_back(i);

   for (std::vector<Frame>::const_iterator
         it = seq.frames.begin() + 1; it != seq.frames.end(); ++it) {
      const int frameIdx = std::distance(seq.frames.begin(), it);
      const Frame& f = *it;

      assert(f.measurements.cols() == f0.measurements.cols());

      Eigen::MatrixXd x1(3, SampleSize);
      Eigen::MatrixXd x2(3, SampleSize);

      int totalEs = 0;
      int totalfound = 0;
      int totalmissed = 0;

      for (int attempt = 0; attempt < SampleCount; ++attempt) {
         // select a random sample (without replacement)
         btl::shuffleMofN(order.begin(), order.begin() + SampleSize, order.end(), std_rng);

         for (int i = 0; i < SampleSize; ++i) {
            x1.col(i) = f0.measurements.col(order[i]);
            x2.col(i) = f.measurements.col(order[i]);
         }

         //std::cout << "Sample points:\n" << x1 << "\n" << x2 << "\n";

         // solve!
         fivePt.compute(x1, x2,
            btl::FivePointStewenius::ApplyRankFix |
            btl::FivePointStewenius::NormaliseResults);

         // check the results
         const int count = fivePt.numSolutions();
         totalEs += count;

         int validcount = 0;
         for (int sol = 0; sol < count; ++sol) {
            Eigen::Matrix3d E = fivePt.solution(sol);

            Eigen::JacobiSVD<Eigen::Matrix3d> svd(E);
            Eigen::Vector3d sv = svd.singularValues();
            //std::cout << "sv = " << sv.transpose() << "\n";
            bool rank2_equal = btl::almost_equal(sv(0), sv(1), CLOSE_ULPS);
            bool rank2_valid = btl::almost_zero(sv(2), CLOSE_TOLERANCE);

            if (!rank2_equal || !rank2_valid)
            {
               std::cout << "!! invalid essential matrix returned from FivePointStewenius\n";
               continue;
            }

            Eigen::VectorXd v = (f0.measurements.transpose() * E * f.measurements).diagonal();

            const double tolerance = 1e-11;
            int numgood = (v.array().abs() <= tolerance).count();
            //std::cout << numgood << "/" << N << " good\n";

            if (numgood == N)
               ++validcount;
         }

         if (validcount == 1)
            ++totalfound;
         else
            ++totalmissed;
      }

      std::cout << "After " << SampleCount << " samples, for frame " << frameIdx << ":\n";
      std::cout << "  totalEs = " << totalEs << "\n";
      std::cout << "  totalfound = " << totalfound << "\n";
      std::cout << "  totalmissed = " << totalmissed << "\n";
   }
}

void testFivePointSteweniusOctave() {
   Eigen::Map<const Eigen::MatrixXd> x1(&DATA_X1[0][0], 3, DATA_POINT_COUNT);
   Eigen::Map<const Eigen::MatrixXd> x2(&DATA_X2[0][0], 3, DATA_POINT_COUNT);

   Eigen::Matrix<double, 3, Eigen::Dynamic> x1s(3, DATA_SAMPLE_SIZE);
   Eigen::Matrix<double, 3, Eigen::Dynamic> x2s(3, DATA_SAMPLE_SIZE);

   for (int i = 0; i < DATA_SAMPLE_SIZE; ++i) {
      x1s.col(i) = x1.col(DATA_SAMPLE[i]);
      x2s.col(i) = x2.col(DATA_SAMPLE[i]);
   }

   btl::FivePointStewenius fivePt;
   fivePt.compute(x1s, x2s);

   const int count = fivePt.numSolutions();
   const int N = DATA_POINT_COUNT;

   std::cout << "FivePointStewenius found " << count << " possible E matrices.\n";

   for (int sol = 0; sol < count; ++sol) {
      Eigen::Matrix3d E = fivePt.solution(sol);

      Eigen::JacobiSVD<Eigen::Matrix3d> svd(E);
      Eigen::Vector3d sv = svd.singularValues();

      std::cout << "sv = " << sv.transpose() << "\n";

      bool rank2_equal = btl::almost_equal(sv(0), sv(1), CLOSE_ULPS);
      bool rank2 = btl::almost_zero(sv(2), CLOSE_TOLERANCE*512.0);

      Eigen::VectorXd v = (x1.transpose() * E * x2).diagonal();

      int numgood = (v.array().abs() <= CLOSE_TOLERANCE).count();
      std::cout << numgood << "/" << N << " good\n";

      bool epipolar = (numgood == N);

      std::cout
         << "rank2: " << (rank2_equal ? "YES" : "NO")
         << "; " << (rank2 ? "YES" : "NO")
         << "; with " << numgood << "/" << N << " fitting epipolar constraint.\n";
   }
}

void initGL() {
   glEnable(GL_DEPTH_TEST);
   glDisable(GL_TEXTURE_2D);
   glDisable(GL_LIGHTING);
   glEnable(GL_LINE_SMOOTH);
   glDisable(GL_CULL_FACE);
   glEnable(GL_BLEND);
   //glBlendFunc(GL_ONE, GL_ONE_MINUS_SRC_ALPHA);
   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

   glClearColor(0.15, 0.15, 0.15, 1.0);
}

Eigen::Vector2f viewPlaneMousePt(int x, int y, double fov) {
   int w, h;
   glfwGetWindowSize(&w, &h);

   const double aspect = double(h) / double(w);
   const double extentx = std::tan(fov/2.0);
   const double extenty = extentx * aspect;

   const double xx = (double( x) * 2.0 / w - 1.0) * extentx;
   const double yy = (double(-y) * 2.0 / h + 1.0) * extenty;

   return Eigen::Vector2f(xx, yy);
}

void initView(const TurntableController& mouseball, double fov) {
   int width, height;
   glfwGetWindowSize(&width, &height);

   glViewport(0,0,width,height);

   const double aspect = double(height) / double(width);
   const double extentx = std::tan(fov/2.0);
   const double extenty = extentx * aspect;

   // draw the axis marker
   // nb: must be done before setting the projection, because
   //     DrawAxisMarker messes with the projection matrix
   DrawAxisMarker(fov).render(
      width, height, mouseball.getViewRotation());

   // set up the projection matrix
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   glFrustum(-extentx, extentx, -extenty, extenty, 1.0, 1000.0);

   // OpenGL expects the view to be along negative Z
   // (whereas we have it along positive Z)
   glScaled(1.0, 1.0, -1.0);

   // set up the view according to the controller
   const Eigen::Projective3d view = mouseball.getWorldToView();
   glMultMatrixd(view.matrix().data());

   glMatrixMode(GL_MODELVIEW);
   glLoadIdentity();
}

void renderWorldPoints(const World& world) {
   // using vertex arrays to render the point data, because there's
   // a lot of them and we already have them in the correct layout
   glEnableClientState(GL_VERTEX_ARRAY);

   // render all the world points
   glVertexPointer(3, GL_DOUBLE, 0, reinterpret_cast<const void*>(world.points.data()));
   glColor4f(1.0f, 1.0f, 0.0f, 1.0f);
   glDrawArrays(GL_POINTS, 0, world.points.cols());

   glDisableClientState(GL_VERTEX_ARRAY);
}

void renderCamera(
      const Eigen::Isometry3d& cameraToWorld,
      const Eigen::Vector4f& col, const Eigen::Vector4f& zcol) {
   DrawCameraMarker marker;
   marker.z = CAMERA_SIZE;
   marker.colour = col;

   glPushMatrix();
   glMultMatrixd(cameraToWorld.data());
   marker.render();

   if (zcol(3) > 0.0f) {
      glColor4f(zcol(0), zcol(1), zcol(2), zcol(3));
      glBegin(GL_LINES);
      glVertex3d(0.0, 0.0, 0.0);
      glVertex3d(0.0, 0.0, CAMERA_SIZE);
      glEnd();
   }

   glPopMatrix();
}

void renderCamera(const Eigen::Isometry3d& cameraToWorld, const Eigen::Vector4f& col) {
   DrawCameraMarker marker;
   marker.z = CAMERA_SIZE;
   marker.colour = col;

   glPushMatrix();
   glMultMatrixd(cameraToWorld.data());
   marker.render();
   glPopMatrix();
}

void renderMotion(
      const Motion& motion,
      const btl::extra::ColourMap& colours,
      const Eigen::Vector4f& zcol = Eigen::Vector4f(0.0f, 0.0f, 0.0f, 0.0f)) {
   for (std::vector<Eigen::Isometry3d>::const_iterator
         poseIt = motion.cameraToWorld.begin(); poseIt != motion.cameraToWorld.end(); ++poseIt) {

      const Eigen::Isometry3d& cameraToWorld = *poseIt;
      const int i = std::distance(motion.cameraToWorld.begin(), poseIt);
      const double x = double(i) / double(motion.cameraToWorld.size() - 1);

      renderCamera(cameraToWorld, colours.at(x), zcol);
   }
}

void renderEssentialDecompositionTest(
      const Motion& motion,
      const btl::extra::ColourMap& colourbar) {

   const Eigen::Isometry3d& cameraToWorld0 = motion.cameraToWorld.front();
   const Eigen::Isometry3d worldToCamera0 = cameraToWorld0.inverse();

   // render the origin camera in white
   renderCamera(cameraToWorld0, Eigen::Vector4f::Ones());

   // render reconstructed cameras
   for (std::vector<Eigen::Isometry3d>::const_iterator
         poseIt = motion.cameraToWorld.begin() + 1;
         poseIt != motion.cameraToWorld.end(); ++poseIt) {
      const Eigen::Isometry3d& cameraToWorld = *poseIt;
      const Eigen::Isometry3d worldToCamera = cameraToWorld.inverse();
      const int frameIdx = std::distance(motion.cameraToWorld.begin(), poseIt);
      const double frameAlpha = double(frameIdx) / double(motion.cameraToWorld.size() - 1);

      // relPose takes you from camera 0 to camera X
      const Eigen::Isometry3d relPose = worldToCamera * cameraToWorld0;
      const Eigen::Matrix3d E = btl::BuildEssentialMatrix(relPose).matrix();

      // render the reconstructed cameras on a spectrum
      Eigen::Vector4f colour = colourbar.at(frameAlpha);

      btl::DecomposeEssentialHorn rtE(E);
      const Eigen::Isometry3d Rt[4] = {
         rtE.transformA(), // red
         rtE.transformB(), // yellow
         rtE.transformC(), // green
         rtE.transformD()  // cyan
      };

      for (int j = 0; j < 4; ++j) {
         Eigen::Vector4f zcol;
         switch (j) {
         case 0: zcol = Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f); break;
         case 1: zcol = Eigen::Vector4f(1.0f, 1.0f, 0.0f, 1.0f); break;
         case 2: zcol = Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f); break;
         case 3: zcol = Eigen::Vector4f(0.0f, 1.0f, 1.0f, 1.0f); break;
         }

         renderCamera(cameraToWorld0 * Rt[j].inverse(), colour, zcol);
      }
   }
}

void renderPointTriangulationTest(
      const Sequence& seq, const btl::extra::ColourMap& colourbar) {

   const Eigen::Isometry3d worldToCamera0 = seq.frames.front().worldToCamera;
   const Eigen::Isometry3d& cameraToWorld0 = worldToCamera0.inverse();
   const Eigen::Matrix3Xd& measurements0 = seq.frames.front().measurements;
   const int N = measurements0.cols();

   const Eigen::Vector3d x1 = measurements0.col(0);

   // render the origin camera in white
   renderCamera(cameraToWorld0, Eigen::Vector4f::Ones());

   for (std::vector<Frame>::const_iterator
         frameIt = seq.frames.begin() + 1;
         frameIt != seq.frames.end(); ++frameIt) {
      const Eigen::Isometry3d& worldToCamera = frameIt->worldToCamera;
      const Eigen::Isometry3d cameraToWorld = worldToCamera.inverse();
      const Eigen::Matrix3Xd& measurements = frameIt->measurements;
      const int frameIdx = std::distance(seq.frames.begin(), frameIt);
      const double frameAlpha = double(frameIdx) / double(seq.frames.size() - 1);

      const Eigen::Vector3d x2 = measurements.col(0);

      // relPose takes you from camera 0 to camera X
      const Eigen::Isometry3d relPose = worldToCamera * cameraToWorld0;
      const Eigen::Matrix3d E = btl::BuildEssentialMatrix(relPose).matrix();

      // render the reconstructed cameras on a spectrum
      Eigen::Vector4f colour = colourbar.at(frameAlpha);

      // decompose E to four relative poses
      btl::DecomposeEssentialHorn rtE(E);

      // select the correct pose based on one point
      btl::DisambiguateEssential calcP(rtE.rotationX(), rtE.rotationY(), rtE.translation(), x1, x2);

      // render the reconstructed camera pose
      const Eigen::Isometry3d P = calcP.pose();
      renderCamera(cameraToWorld0 * P.inverse(), colour);

      // render the reconstructed disambiguation point
      glColor4f(colour(0), colour(1), colour(2), 0.5f);
      glPointSize(5.0);
      glDisable(GL_DEPTH_TEST);
      glBegin(GL_POINTS);
      for (int i = 0; i < N; ++i) {
         Eigen::Vector4d rp = btl::triangulate(P, calcP.E(), measurements0.col(i), measurements.col(i));
         rp = cameraToWorld0.matrix() * rp;
         glVertex4d(rp(0), rp(1), rp(2), rp(3));
      }
      glEnd();
      glPointSize(1.0);
      glEnable(GL_DEPTH_TEST);
   }
}

void findCommonIndexes(std::vector<int>& common, const std::vector<int>& a, const std::vector<int>& b) {
   common.clear();
   common.reserve(std::min(a.size(), b.size()));

   std::vector<int>::const_iterator idxA = a.begin();
   std::vector<int>::const_iterator idxB = b.begin();
   while (idxA != a.end() && idxB != b.end()) {
      if (*idxA == *idxB) {
         common.push_back(*idxA);
         ++idxA;
         ++idxB;
      } else {
         if (*idxA < *idxB) ++idxA;
         if (*idxB < *idxA) ++idxB;
      }
   }
}

void renderFivePointFrame(
      const World& world,
      const Frame& frame0,
      const Frame& frame1,
      boost::mt19937& rng,
      const Eigen::Vector4f& colour) {

   const Eigen::Isometry3d& worldToCamera0 = frame0.worldToCamera;
   const Eigen::Isometry3d cameraToWorld0 = worldToCamera0.inverse();
   const Eigen::Matrix3Xd& measurements0 = frame0.measurements;
   const int N = measurements0.cols();

   boost::random_number_generator<boost::mt19937> std_rng(rng);

   const int SampleSize = 8;
   const int VerifierCount = 40; // how many points to verify against
   const int SampleCount = 500;

   const Eigen::Vector4f Green = Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f);
   const Eigen::Vector4f Yellow = Eigen::Vector4f(1.0f, 1.0f, 0.0f, 1.0f);

   // should be able to test a few hundred samples at frame-rate,
   // otherwise RANSAC on 5-point-pose won't really work very well...

   std::vector<int> commonIndexes;
   findCommonIndexes(commonIndexes, frame0.indexes, frame1.indexes);

   std::cout << commonIndexes.size() << " possible matches.\n";

   btl::FivePointStewenius fivePt;

   // reconstruct camera1 and render it
   const Eigen::Isometry3d& worldToCamera1 = frame1.worldToCamera;
   const Eigen::Isometry3d cameraToWorld1 = worldToCamera1.inverse();

   // relPose takes you from camera 1 to camera 0
   const Eigen::Isometry3d relPose = worldToCamera0 * cameraToWorld1;
   const Eigen::Matrix3Xd& measurements1 = frame1.measurements;
   const double baselineLength = relPose.translation().norm();

   Eigen::MatrixXd x1(3, SampleSize);
   Eigen::MatrixXd x2(3, SampleSize);

   for (int attempt = 0; attempt < SampleCount; ++attempt) {
      btl::shuffleMofN(
         commonIndexes.begin(),
         commonIndexes.begin() + (SampleSize + VerifierCount),
         commonIndexes.end(),
         std_rng);

      for (int i = 0; i < SampleSize; ++i) {
         x1.col(i) = measurements0.col(commonIndexes[i]);
         x2.col(i) = measurements1.col(commonIndexes[i]);
      }

      //std::cout << "x1:\n" << x1 << "\nx2:\n" << x2 << "\n";

      fivePt.compute(x1, x2);

      double errorVs[10];

      const int solutionCount = fivePt.numSolutions();
      for (int sol = 0; sol < solutionCount; ++sol) {
         const Eigen::Matrix3d& E = fivePt.solution(sol);

         errorVs[sol] = 0.0;

         // test a bunch of other points to see if it's really a valid E
         for (int i = 0; i < (SampleSize + VerifierCount); ++i) {
            const double v
               = measurements1.col(commonIndexes[i]).transpose()
               * E * measurements0.col(commonIndexes[i]);

            errorVs[sol] += std::abs(v);
         }
      }

      //std::cout << "Errors: ";
      //std::copy(&errorVs[0], &errorVs[solutionCount], std::ostream_iterator<double>(std::cout, " "));
      //std::cout << "\n";

      double minError = 1e100;
      int bestSol = -1;
      for (int sol = 0; sol < solutionCount; ++sol) {
         if ((bestSol == -1) || (errorVs[sol] < minError)) {
            bestSol = sol;
            minError = errorVs[sol];
         }
      }

      if (minError < 1e-2 * double(SampleSize + VerifierCount)) {
         Eigen::Matrix3d validE = fivePt.solution(bestSol);

         // decompose E to four relative poses
         btl::DecomposeEssentialHorn rtE(validE,
            btl::DecomposeEssentialHorn::ComputeNormalisedT |
            btl::DecomposeEssentialHorn::ComputeNormalisedE);
         const Eigen::Matrix3d& normE = rtE.normalisedE();

         // select the correct pose based on one point
         btl::DisambiguateEssential calcP(
            rtE.rotationX(), rtE.rotationY(), rtE.translation(),
            x1.col(0), x2.col(0));
         const Eigen::Isometry3d& Rt = calcP.pose();

         // apply our known baseline
         Eigen::Isometry3d reconstructedRelPose
            = Eigen::Translation3d(baselineLength * Rt.translation())
            * Eigen::Isometry3d(Rt.rotation());

         Eigen::Vector4f col;
         if (minError < 0.5e-2 * double(SampleSize + VerifierCount))
            col = Green;
         else
            col = Yellow;

         renderCamera(cameraToWorld0 * reconstructedRelPose.inverse(), col);
      }

      //std::cout << numvalid << "/" << solutionCount << " solutions.\n";
   }
}

void renderFivePointTest(const Sequence& seq, const World& world, const btl::extra::ColourMap& colourbar) {
   const Frame& frame0 = seq.frames.front();

   // render the origin camera in white
   renderCamera(frame0.worldToCamera.inverse(), Eigen::Vector4f::Ones());

   boost::mt19937 rng;

   // render cameras reconstructed with FivePointStewenius
   for (std::vector<Frame>::const_iterator
         frameIt = seq.frames.begin() + 1;
         frameIt != seq.frames.end(); ++frameIt) {
      const int frameIdx = std::distance(seq.frames.begin(), frameIt);
      const double frameAlpha = double(frameIdx) / double(seq.frames.size() - 1);

      renderFivePointFrame(world, frame0, *frameIt, rng, colourbar.at(frameAlpha));
   }
}

void renderFivePointAnimatedTest(
      const World& world,
      const Frame& frame0,
      const Eigen::Isometry3d& cameraToWorldN,
      const SimCamera& simCamera,
      double alpha, const btl::extra::ColourMap& colourbar,
      boost::mt19937& rng) {

   assert(alpha >= 0.0 && alpha <= 1.0);

   const Eigen::Isometry3d cameraToWorld0 = frame0.worldToCamera.inverse();

   // render the origin camera in white
   renderCamera(cameraToWorld0, Eigen::Vector4f::Ones());

   Eigen::Isometry3d cameraN = lerpTransform(alpha, cameraToWorld0, cameraToWorldN);
   Frame frameN = captureFrame(world, cameraN, simCamera, rng);

   renderFivePointFrame(world, frame0, frameN, rng, colourbar.at(alpha));
}

int main(int argc, char** argv) {
   try {
      boost::mt19937 rng(361385024);
      boost::mt19937 animationRng;

      std::cout << "Generating randomised landmarks...\n";

      // generate a random world with N landmarks
      const int N = 1000;
      const Eigen::Vector3d minCoord(-10.0, -10.0, 10.0);
      const Eigen::Vector3d maxCoord( 10.0,  10.0, 20.0);
      World world = generateWorldUniformBox(N, minCoord, maxCoord, rng);

      std::cout << "Simulating motion...\n";

      // generate a simple linear camera motion
      const Eigen::Isometry3d
         camera0
            = Eigen::Translation3d(-5.0, 0.0, 0.0)
            * Eigen::Isometry3d(Eigen::AngleAxisd(
               10.0*RADIANS_PER_DEGREE,
               Eigen::Vector3d::UnitY())
            );
      const Eigen::Isometry3d
         camera1
            = Eigen::Translation3d( 5.0, 0.0, 0.0)
            * Eigen::Isometry3d(Eigen::AngleAxisd(
               -35.0*RADIANS_PER_DEGREE,
               Eigen::Vector3d::UnitY())
            );

      SimCamera simCamera;
      simCamera.resX = 752;
      simCamera.resY = 480;
      simCamera.fovX = CAMERA_FOV;
      // sigma = 0.25
      // this corresponds with 95.5% of measurements being less than 0.5 pixels away
      // from the true position. this is roughly the same as saying that 95.5% of
      // features will be measured correctly to the nearest pixel
      simCamera.measurementNoise = 0.25;

      Motion motion = generateLinearMotion(20, camera0, camera1);

      std::cout << "Simulating measurements...\n";

      // take pictures!
      Sequence seq = captureSequence(world, motion, simCamera, rng);

      // run tests on the sequence
      //testGaussJordanEliminationOctave();
      //testBuildEssential(seq);
      //testDecomposeEssential(motion);
      //testFivePointSteweniusOctave();
      //testFivePointStewenius(seq, rng);

      glfwInit();
      glfwOpenWindow(800, 600, 8, 8, 8, 8, 16, 0, GLFW_WINDOW);
      glfwEnable(GLFW_STICKY_KEYS);

      initGL();

      TurntableController mouseball;
      const double VIEW_FOV = 60.0 * RADIANS_PER_DEGREE;

      bool running = true;
      int wheelPos = glfwGetMouseWheel();

      const double motionPeriod = 3.0; // 3 seconds for the motion
      const double loopPeriod = 4.0; // 4 seconds for the whole loop
      double startTime = glfwGetTime();
      while (running) {
         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
         initView(mouseball, VIEW_FOV);
         renderWorldPoints(world);
         //renderMotion(motion, btl::extra::ColourMap::ConstantWhite);
         //renderEssentialDecompositionTest(motion, btl::extra::ColourMap::Jet);
         //renderPointTriangulationTest(seq, btl::extra::ColourMap::Jet);
         //renderFivePointTest(seq, world, btl::extra::ColourMap::Jet);

         double thisTime = glfwGetTime();
         double deltaT = (thisTime - startTime);
         double alpha = std::min(deltaT / motionPeriod, 1.0);
         if (deltaT > loopPeriod) {
            deltaT = loopPeriod;
            startTime = thisTime;
         }

         renderFivePointAnimatedTest(
            world, seq.frames.front(), camera1, simCamera,
            alpha, btl::extra::ColourMap::Jet, animationRng);

         glfwSwapBuffers();

         // no point burning CPU since we're not doing any animation
         //glfwWaitEvents();

         // process input
         int x, y;
         glfwGetMousePos(&x, &y);
         const Eigen::Vector2f mousePt = viewPlaneMousePt(x, y, VIEW_FOV);

         const bool leftDown = bool(glfwGetMouseButton(GLFW_MOUSE_BUTTON_LEFT));
         const bool rightDown = bool(glfwGetMouseButton(GLFW_MOUSE_BUTTON_RIGHT));
         const bool ctrlDown = glfwGetKey(GLFW_KEY_LCTRL) || glfwGetKey(GLFW_KEY_RCTRL);

         const int wheelDelta = glfwGetMouseWheel() - wheelPos;
         wheelPos += wheelDelta;

         if (mouseball.isDragging()) {
            if (!leftDown && !rightDown)
               mouseball.endDrag(mousePt);
            else
               mouseball.updateDrag(mousePt);
         } else {
            if (leftDown)
               mouseball.beginDragRotate(mousePt);
            else if (rightDown) {
               if (!ctrlDown)
                  mouseball.beginDragLookat(mousePt);
               else
                  mouseball.beginDragLookatZ(mousePt);
            }
         }

         if (wheelDelta)
            mouseball.scrollRange(wheelDelta*120);

         running =
            !glfwGetKey(GLFW_KEY_ESC) && glfwGetWindowParam(GLFW_OPENED);
      }
      glfwTerminate();

      return 0;
   } catch (std::exception& e) {
      std::cerr << "Uncaught exception: " << e.what() << "\n";
      return 1;
   } catch (...) {
      std::cerr << "Uncaught exception: (unknown)\n";
      return 1;
   }
}
